/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "MaCI_OccuMapper.h"
//#define USE_MACI_OCCUMAPPER_DEBUG


using namespace MaCI;
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/// Constructors and destructors
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////

CMaCIOccuMapper::CMaCIOccuMapper(std::string gimnetap, 
				 int gimnetapport, 
				 std::string gimnetname,
				 std::string macigroup,
				 std::string positionserverinstancename,
				 std::string rangingserverinstancename,
				 double occumapsize,
				 double occumapresolution,
				 float edgethreshold,
				 int numberofscanpoints,
				 float max_laser_dist,
				 float min_laser_dist) 
  : CSync(1,3),
    rangingServer(NULL),
	gimi_ptr(NULL),
	gimi_ptr2(NULL),
	MSC(NULL),
	MSC2(NULL)
	
{
  initializeExternalVariables(gimnetap,gimnetapport,gimnetname,macigroup,
			      positionserverinstancename,rangingserverinstancename,
			      occumapsize,occumapresolution,edgethreshold,numberofscanpoints,max_laser_dist,
			      min_laser_dist);
  initializeInternalVariables();
		
		
}
CMaCIOccuMapper::CMaCIOccuMapper(gimi::GIMI *Gptr,
				 MaCI::MaCICtrl::TServiceEntryArray &services_,
				 std::string macigroup,
				 std::string positionserverinstancename,
				 std::string rangingserverinstancename,
				 double occumapsize,
				 double occumapresolution,
				 float edgethreshold,
				 int numberofscanpoints,
				 float max_laser_dist,
				 float min_laser_dist
				 ) 
  : CSync(1,3),
    rangingServer(NULL),
    gimi_ptr(NULL),
    MSC(NULL)
{
  if(Gptr == NULL){
    dPrint(1,"Provided GIMI pointer must be initialized!! Aborting");
    exit(1);
  }else{
    gimi_ptr = Gptr;
  }
		
  if(!gimi_ptr->isConnected()){
    dPrint(1,"Provided GIMI pointer must be Connected to a tcpHUB!! Aborting");
    exit(1);
  }
		
  initializeExternalVariables(gimi_ptr->getHubAddress(),
			      gimi_ptr->getHubPort(),
			      "doesnotmatter" ,
			      macigroup,
			      positionserverinstancename,
			      rangingserverinstancename,
			      occumapsize,
			      occumapresolution,
			      edgethreshold,
			      numberofscanpoints,
			      max_laser_dist,
			      min_laser_dist);
  gimi_ptr = Gptr; ///< initializeExternalVariables sets the pointer to NULL!
  initializeInternalVariables(services_);
		
}


/**
 * Constructor with initilized GIMI,  MaCI service list and with a settings file
 * @param *Gptr initialized and connected GIMI ptr
 * @param *&services_ MaCI service list 
 * @param macigroup The MaCI Group name of this module, as well as the position and ranging sources
 * @param positionserverinstancename The instance name of the position server (e.g. "Motion")
 * @param rangingserverinstancename The instance name of ranging server (e.g. "Laser")
 * @param settings_file_name
 */
CMaCIOccuMapper::CMaCIOccuMapper(gimi::GIMI *Gptr,
				 MaCI::MaCICtrl::TServiceEntryArray &services_,
				 std::string macigroup,
				 std::string positionserverinstancename,
				 std::string rangingserverinstancename,
				 std::string settings_file_name) : CSync(1,3)
{
  dPrint(2,"********************************************************");
  dPrint(2,"OccuMapper!");
  dPrint(2,"********************************************************");
		
  if(Gptr == NULL){
    dPrint(1,"Provided GIMI pointer must be initialized!! Aborting");
    exit(1);
  }else{
    gimi_ptr = Gptr;
  }
		
  if(!gimi_ptr->isConnected()){
    dPrint(1,"Provided GIMI pointer must be Connected to a tcpHUB!! Aborting");
    exit(1);
  }
  float occumapsize,occumapresolution,edgethreshold,max_laser_dist,min_laser_dist;
  int numberofscanpoints;
		
  ///Open & read settings file
  gim::CSettings settings;
  if(!settings.ReadSettingsXMLFile(settings_file_name)){
    dPrint(1,"Failed to read the configuration file!!! - Aborting");
    exit(1);
  }
  bool result=true;
  result &= settings.GetValue("OccuMapper.grid_map_size",occumapsize);
  result &= settings.GetValue("OccuMapper.resolution",occumapresolution);
  result &= settings.GetValue("OccuMapper.toEdge",edgethreshold);
  result &= settings.GetValue("OccuMapper.max_laser_distance",max_laser_dist);
  result &= settings.GetValue("OccuMapper.min_laser_distance",min_laser_dist);
  result &= settings.GetValue("OccuMapper.number_of_measurements",numberofscanpoints);
		
  if(!result){
    dPrint(1,"ERROR: Failed to read Occumapper Settings!!! - Abort()!");
    exit(1);
  }
		
  initializeExternalVariables(gimi_ptr->getHubAddress(),
			      gimi_ptr->getHubPort(),
			      "doesnotmatter" ,
			      macigroup,
			      positionserverinstancename,
			      rangingserverinstancename,
			      occumapsize,
			      occumapresolution,
			      edgethreshold,
			      numberofscanpoints,
			      max_laser_dist,
			      min_laser_dist);
  gimi_ptr = Gptr; ///< initializeExternalVariables sets the pointer to NULL!
		
		
  initializeInternalVariables(services_);
}



CMaCIOccuMapper::~CMaCIOccuMapper(){
  if(threadRunning){
    stopOccuMapperExecution();
  }
		
  //if(gmapdata) free(gmapdata);
  if(lmapdata) free(lmapdata);
  if(localCharMap) free(localCharMap);
  if(globalCharMap) free(globalCharMap);

}
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/// Initialization (PRIVATE)
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/**
 * Initializes the external (given) variables.
 **/
void CMaCIOccuMapper::initializeExternalVariables(std::string gimnetap, 
						  int gimnetapport, 
						  std::string gimnetname,
						  std::string macigroup,
						  std::string positionserverinstancename,
						  std::string rangingserverinstancename,
						  double occumapsize,
						  double occumapresolution,
						  float edgethreshold,
						  int numberofscanpoints,
						  float max_laser_dist,
						  float min_laser_dist)
{
  gimnetAP=gimnetap;
  gimnetAPPort=gimnetapport;
  gimnetName=gimnetname;
  maciGroup=macigroup;
  positionServerInstanceName=positionserverinstancename;
  rangingServerInstanceName=rangingserverinstancename;
		
  occu_map_size=occumapsize;
  occu_map_resolution=occumapresolution;
  enough_scan_points=numberofscanpoints;
  EdgeThreshold=edgethreshold;
  max_laser_distance = max_laser_dist;
  min_laser_distance = min_laser_dist;
		
  sendLocalMaps=false;
  localMapSize=100;
  lmapdata=NULL;
  //gmapdata=NULL;
		
  /**
   * Set all access related objects to NULL
   */
  gimi_ptr = NULL;
  gimi_ptr2 = NULL;
  MSC = NULL;
  MSC2 = NULL;
  positionClient = NULL;
  imageServer = NULL;
  picbuf = NULL;
  rgbi=NULL;
  mapServer = NULL;
  localCharMap = NULL;
  globalCharMap = NULL;
}
/**
 * Initializes all internal variables 
 * Run after setting the externals!!
 * After running this you should have active GIMI connection 
 * as well as active position and ranging client
 **/
void CMaCIOccuMapper::initializeInternalVariables(){
  imageServerActivated = false;
  mapServerActivated = false;
  bSendMotionRanges = false;
		
  if(gimi_ptr == NULL){
	  gimi_ptr = new gimi::GIMI(); ///< Create GIMI
	  int r;
	  
	  /// Connect to GIMnet
	  if((r = gimi_ptr->connectToHubEx(gimnetAP, gimnetAPPort, gimnetName)) != GIMI_OK){
		  dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
				  gimnetAP.c_str(),gimnetAPPort,gimnetName.size()?gimnetName.c_str():"<anonymous>",
																					gimi::getGimiErrorString(r).c_str());
																					exit(1);
	  }else{
		  dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
				  gimnetAP.c_str(), 
				  gimnetAPPort, 
				  gimnetName.size()?gimnetName.c_str():"<anonymous>",
													  gimi::getGimiErrorString(r).c_str());
	  }
  }
  
  if(gimi_ptr2 == NULL){
	  gimi_ptr2 = new gimi::GIMI(); ///< Create GIMI
	  int r;
	  std::string second = "L";
	  std::string gimnetName2 = gimnetName.size()?gimnetName.c_str() + second:"";
	  /// Connect to GIMnet
	  if((r = gimi_ptr2->connectToHubEx(gimnetAP, gimnetAPPort, gimnetName2)) != GIMI_OK){
		  dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
				  gimnetAP.c_str(),gimnetAPPort,gimnetName2.size()?gimnetName2.c_str():"<anonymous>",
																					gimi::getGimiErrorString(r).c_str());
																					exit(1);
	  }else{
		  dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
				  gimnetAP.c_str(), 
				  gimnetAPPort, 
				  gimnetName2.size()?gimnetName2.c_str():"<anonymous>",
													  gimi::getGimiErrorString(r).c_str());
	  }
  }
  /////////////////////////////////////////////////////////////////////////////
  /// GET ALL MACI SERVICES FROM GIMNET
  /////////////////////////////////////////////////////////////////////////////
  dPrint(1,"Aquiring the service list!");
  MaCI::EMaCIError e;
  MaCICtrl::CMaCICtrlClient ctrlclient(gimi_ptr); ///< Open MaCICtrl client.
		
  if (ctrlclient.Open() == KMaCIOK) {
    // Get all MaCI services from local GIMnet.
    gimi::t_serviceList macictrlservicelist;
    if (ctrlclient.DiscoverMaCICtrlServices(macictrlservicelist,5000) == KMaCIOK) {	
      // Request information from all MaCI nodes on the Net.			
      if (ctrlclient.GetServiceArray(services, macictrlservicelist, 5000) != MaCI::KMaCIOK) {
	dPrint(1,"Failed to get service array - aborting");
	exit(1);
      }
    }else{
      dPrint(1,"Failed to receive MaCI services from the GIMnet - Aborting - We are Wery sorry!");
      exit(1);
    }
  }else{
    dPrint(1,"Error: Failed to open MaCI Control Client! Abort!");
    exit(1);
  }
  /////////////////////////////////////////////////////////////////////////////
  /// Open Clients
  /////////////////////////////////////////////////////////////////////////////
  MaCI::Ranging::CRangingClient *rangingClient=new MaCI::Ranging::CRangingClient(gimi_ptr);
  positionClient = new MaCI::Position::CPositionClient(gimi_ptr);
		
  std::string positionServerName = maciGroup + ".MaCI_Position."+positionServerInstanceName;
		
  positionClient->SetDefaultTarget(
				   MaCICtrl::SMaCISL(positionServerName),services);	
		
  std::string rangingServerName = maciGroup + ".MaCI_Ranging."+rangingServerInstanceName;
  rangingClient->SetDefaultTarget(
				  MaCICtrl::SMaCISL(rangingServerName),services);
		
  if (positionClient->Open() != MaCI::KMaCIOK) {
    dPrint(1,"Error: Unable to open Position Client to '%s'",positionServerName.c_str());
    exit(1);
  }
  if (rangingClient->Open() != MaCI::KMaCIOK) {
    dPrint(1,"Error: Unable to open Ranging Client to '%s'",rangingServerName.c_str());
    exit(1);
  }
		
  rangingClients.push_back(rangingClient);
		
  grid_scan.alloc(enough_scan_points);
  O = new TOccupancyGrid(occu_map_size,occu_map_size,occu_map_resolution,Grid::TCoord2D(0,0));
  O->setGridToValue(0.5);
  if(!RunThread()){
    dPrint(1,"Unable to start the mapper thread");
    exit(1);
  }
		
		
}

/**
 * Initialize the variables and start the thread
 * Uses Initialized GIMI and MaCI service list 
 */
void CMaCIOccuMapper::initializeInternalVariables(MaCI::MaCICtrl::TServiceEntryArray &services_)
{
  imageServerActivated = false;
  mapServerActivated = false;
  bSendMotionRanges = false;
		
  services = services_; ///< copy the services to a local variable
  /////////////////////////////////////////////////////////////////////////////
  /// Open Clients
  /////////////////////////////////////////////////////////////////////////////
  MaCI::Ranging::CRangingClient *rangingClient=new MaCI::Ranging::CRangingClient(gimi_ptr);
		
  positionClient = new MaCI::Position::CPositionClient(gimi_ptr);
		
  std::string positionServerName = maciGroup + ".MaCI_Position."+positionServerInstanceName;
		
  positionClient->SetDefaultTarget(
				   MaCICtrl::SMaCISL(positionServerName),services_);	
		
  std::string rangingServerName = maciGroup + ".MaCI_Ranging."+rangingServerInstanceName;
  rangingClient->SetDefaultTarget(
				  MaCICtrl::SMaCISL(rangingServerName),services_);
		
  if (positionClient->Open() != MaCI::KMaCIOK) {
    dPrint(1,"Error: Unable to open Position Client to '%s'",positionServerName.c_str());
    exit(1);
  }
  if (rangingClient->Open() != MaCI::KMaCIOK) {
    dPrint(1,"Error: Unable to open Ranging Client to '%s'",rangingServerName.c_str());
    exit(1);
  }
		
  rangingClients.push_back(rangingClient);
  grid_scan.alloc(enough_scan_points);
  O = new TOccupancyGrid(occu_map_size,occu_map_size,occu_map_resolution,Grid::TCoord2D(0,0));
  O->setGridToValue(0.5);
  if(!RunThread()){
    dPrint(1,"Unable to start the mapper thread");
    exit(1);
  }
}


/**
 * Activates the Map Servers
 * The map server provides the mapped map as an occupancy grid
 * @param instanceName The instance name of the server
 * @param sendLocalMaps if true the map server will send local maps
 * @param lmapsize How big local map is read around the robot
 * @return true is success
 **/
bool CMaCIOccuMapper::activateMapServer(std::string instanceName,
					bool _sendLocalMaps, 
					float lmapsize)
{
		
  mapServerInstanceName=instanceName;
  sendLocalMaps=_sendLocalMaps;
  localMapSize=lrint(lmapsize/O->getResolution());
  lmapdata=(float *)malloc(localMapSize*localMapSize*sizeof(float));
		
  localCharMap = (unsigned char *) malloc(localMapSize*localMapSize);
  globalCharMap = (unsigned char *) malloc(O->size());
		
  mapSendingFrequency = 5;
		
  mapData.CreateInternalBinBag();
  gmapData.CreateInternalBinBag();
  ///Do we need to initialize the MaCI CTRL server
  if(MSC == NULL){
    MSC = new MaCI::MaCICtrl::CMaCICtrlServer(gimi_ptr);
    MSC->SetGroupName(maciGroup);
  }
  mapServer=new MaCI::Map::CMapServer(MSC,0,"Map data");
  mapServer->SetInterfaceInstanceName(mapServerInstanceName);		
  if(mapServer->Open() != MaCI::KMaCIOK){
    dPrint(1,"Failed to open Map Server");
    mapServerActivated = false;
    return false;
  }else{
    dPrint(1,"Map Server Succesfully Activated");
    mapServerActivated = true;
    return true;
  }
		
  return true;
}
/**
 * Activates the Ranging Server
 * The map server provides the mapped map as an occupancy grid
 * @param instanceName The instance name of the server
 * @return true is success
 **/
bool CMaCIOccuMapper::activateRangingServer(std::string instanceName){
  dPrint(1,"Activating Ranging Server");
  //MaCI::Ranging::CRangingServer *rangingServer; 
  MaCI::Ranging::TDeviceInformationPosition dPose;
  MaCI::Ranging::TDeviceInformationLimits dLimits;
  

  OwnRangingServerInstanceName=instanceName;
  rangingClients[0]->GetDeviceLimits(dLimits);
  rangingClients[0]->GetDevicePosition(dPose);
  
  ///Do we need to initialize the MaCI CTRL server
  if(MSC2 == NULL){
    MSC2 = new MaCI::MaCICtrl::CMaCICtrlServer(gimi_ptr2);
    MSC2->SetGroupName(maciGroup);
  }

  rangingServer=new MaCI::Ranging::CRangingServer(MSC2,0,"motion filtered ranging");
  rangingServer->SetInterfaceInstanceName(OwnRangingServerInstanceName);
  rangingServer->SetDeviceLimits(dLimits.min_distance, dLimits.min_distance);
  rangingServer->SetDevicePosition(dPose.x, dPose.y, dPose.z, dPose.heading);
  if(rangingServer->Open() != MaCI::KMaCIOK){
    dPrint(1,"Failed to open Ranging Server");
    bSendMotionRanges= false;
    return false;
  }else{
    dPrint(1,"Ranging Server Succesfully Activated");
    bSendMotionRanges = true;
    return true;
  }
}


/**
 * Activates the Image Server. The server plots the map to image (RGB or GRAYSCALE), 
 * and additionally displays the laser and pose of the robot in the map
 * @param instanceName The instance name of the image server
 * @param useRGBimage if true the map is copied as RGB, else as grayscale (to save some space)
 * @param visualizeLaser if true the laser and pose will be visualized also
 * @return  true if success, else false
 */
bool CMaCIOccuMapper::activateImageServer(std::string instanceName, 
					  bool useRGBimage, 
					  bool visualizeLaser,
					  float laserPixSize,
					  bool visualizePose, 
					  float robotRad){
		
  imageServerInstanceName=instanceName;        ///< Instance Name for the server 
  isRGBenabled=useRGBimage;                          ///< Enable RGB (if disabled gray scale is used)
  laserVisualizationEnabled=visualizeLaser;             ///< Enable/Disable laser plotting
  imageData.CreateInternalBinBag();
  imageSendingFrequency = 5;
  poseEnabled=visualizePose;
  robotRadius=robotRad;
  laserHitSize=laserPixSize;
		
  ///Do we need to initialize the MaCI CTRL server
  if(MSC == NULL){
    MSC = new MaCI::MaCICtrl::CMaCICtrlServer(gimi_ptr);
    MSC->SetGroupName(maciGroup);
				
  }
		
  imageServer=new MaCI::Image::CImageServer(MSC,0,"Map Data provided as Image",imageServerInstanceName);
  imageServer->SetSourceInfo(MaCI::Image::TSourceInfo(0,
						      "Map data as image",
						      "This interface provides the Occupancy Grid map as an image"));
		
  if(imageServer->Open() != MaCI::KMaCIOK){
    dPrint(1,"Failed to open Image Server");
    imageServerActivated = false;
    return false;
  }else{
    dPrint(1,"Image Server Succesfully Activated");
    imageServerActivated = true;
    return true;
  }
  return true;
}



/**
 * Sends an occupancy Grid as JPEG Image through image interface
 **/
bool CMaCIOccuMapper::sendMapImage(Grid::pose &pos){						
  bool result;
  int Nx,Ny;
  MaCI::Image::CImageContainer img; 
		
  O->getDim(Nx,Ny);
  float *ptr = O->getDataPtr();
		
		
  if(!isRGBenabled){ ///< set the grayscale map
    if(picbuf == NULL) picbuf = (char *) malloc(Nx*Ny);
    Lock();
    for(int i=0;i<Nx*Ny;i++){ 
      picbuf[i] = (char)(255 * ptr[i]);	
    }
    Unlock();
				
    result = encoder.SetGrayscaleDataPtr(picbuf, Nx,Ny);
  }else{ ///< set RGB map
    if(rgbi==NULL) rgbi = new RGBUtil::RGBImage(Nx,Ny);
    Lock();
    rgbi->set(O->getDataPtr(),0,0,255);
    Unlock();
    if(poseEnabled){
      Grid::TPixel pix = O->toPixel(Grid::TCoord2D(pos.x,pos.y));
      rgbi->setColor(0,128,0);
      rgbi->setFilledCircle(pix.x, pix.y, robotRadius, O->getResolution());
      rgbi->setCoordinateFrame(pix.x,pix.y,pos.a,1.0,O->getResolution());
    }

    if(laserVisualizationEnabled){
      for(int i=0;i<grid_scan.N;i++){
	rgbi->setColor(255,0,0);
	Grid::TPixel pix = O->toPixel(Grid::TCoord2D(
						     pos.x+grid_scan.r[i]*cos(grid_scan.a[i]+pos.a) ,
						     pos.y+grid_scan.r[i]*sin(grid_scan.a[i]+pos.a)));	
	if(laserHitSize>0)	rgbi->setCircle(pix.x,pix.y,laserHitSize,O->getResolution());
	else rgbi->setPixel(pix.x, pix.y);
      }
    }
				
    Lock(2);
    for(int i=0;i<drawpixels.size();i++){
      rgbi->setColor(drawpixels[i].r,drawpixels[i].g,drawpixels[i].b);
      Grid::TPixel pix = O->toPixel(Grid::TCoord2D(drawpixels[i].x-grid_pose.x,
						   drawpixels[i].y-grid_pose.y));
      rgbi->setPixel(pix.x, pix.y);
    }
    Unlock(2);
				
				
    result = encoder.SetRGBDataPtr((const char *)rgbi->getDataPtr(), Nx,Ny);
  }
		
		
  if(result){
    img.SetImageData((const unsigned char*)encoder.GetJPEGDataPtr(), 
		     encoder.GetJPEGDataSize(), 
		     MaCI::Image::KImageDataJPEG,
		     Nx, Ny, 
		     MaCI::Image::KImageDataUnknown,
		     false); // dont COPY!
  }else{
    return false;
  }
  //fprintf(stderr,"Image (%d,%d) = %d?\n",Nx,Ny,encoder.GetJPEGDataSize());
		
		
  imageData.Reset();
  bool r = imageData.SetTimestamp(Common::TTimestamp()); // Set time to current.
  r &= imageData.AddImage(0, img);
  assert(r == true);
  bool e = imageServer->SendImageData(imageData);
  assert(e == KMaCIOK);
		
  return (result & r & e);
}

/**
 * Send Map
 * @param &pos pose of the grid origin 
 **/
bool CMaCIOccuMapper::sendMap(Grid::pose &pos, Grid::pose &rel_pos){
		
  Map::TMapHeader mapHeader;
  Map::TMapUpdateHeader updateHeader;
  Map::TGridBlockHeader blockHeader;
  mapHeader.gridDataResolution=O->getResolution();
  Lock();
  if(sendLocalMaps){
    Grid::TPixel subCenter=O->toPixel(Grid::TCoord2D(rel_pos.x,rel_pos.y));
				
    O->getSubmap(lmapdata, subCenter, localMapSize, localMapSize);
				
    for(int i=0;i<localMapSize*localMapSize;i++){
      localCharMap[i] = (unsigned char) (255*lmapdata[i]);
    }
				
    updateHeader.clear=false;
				
    blockHeader.x=current_pose.x;
    blockHeader.y=-current_pose.y; ///< why negative?
    blockHeader.samples_x=localMapSize;
    blockHeader.samples_y=localMapSize;
    blockHeader.dataOriginX=localMapSize/2;
    blockHeader.dataOriginY=localMapSize/2;
				
    blockHeader.dataType=MaCI::Map::KGridTypeOccupancy; 
    blockHeader.dataSize=localMapSize*localMapSize*sizeof(char);
    mapData.Reset();
    mapData.SetMapUpdateHeader(updateHeader);
    mapData.AddGridBlock(blockHeader,(const char *) localCharMap );
    mapServer->SendMapUpdateEvent(mapData);
  }
  int Nx,Ny;
  O->getDim(Nx,Ny);
		
  MaCI::Map::TGridBlockHeader head;   
  head.dataType=MaCI::Map::KGridTypeOccupancy; 
  head.dataOriginX=Nx/2;
  head.dataOriginY=Ny/2;
  head.samples_x=Nx;
  head.samples_y=Ny;
  head.x=pos.x;
  head.y=-pos.y;
  head.dataSize=Nx*Ny*sizeof(char);
		
  gmapData.Reset();
  gmapData.SetMapHeader(mapHeader);
		
  float *ptr=O->getDataPtr();
  for(int i=0;i<O->size();i++){
    globalCharMap[i] =(unsigned char) (255 * ptr[i]);
  }
		
  gmapData.AddGridBlock(head, (const char *) globalCharMap);	
  Unlock();
		
  mapServer->SetMap(gmapData);
		
  return true;
}

/**
 * Send Ranges
 * @param &beam_count number of entries in "beams" 
 * @param &beams pointer to table of TDistance pointers
 **/

bool CMaCIOccuMapper::sendRanges(uint32_t beam_counti, const MaCI::Ranging::TDistance **beams){
	
  return false;
}			




///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/// Access methods
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/**
 * Returns the latest pose received from the positionClient
 */
void CMaCIOccuMapper::getCurrentPose(Grid::pose &pose){
  Lock(1);
  pose = current_pose;
  Unlock(1);
}

/**
 * Return the latest pose and laser measurements
 **/
void CMaCIOccuMapper::getCurrentMeasurements(Grid::pose &pose, Grid::TScan &scan){
  if(scan.N < grid_scan.N) scan.alloc(grid_scan.N);
  Lock(1);
  pose = current_pose;
		
  for(int i=0;i<grid_scan.N;i++){
    scan.r[i] = grid_scan.r[i];
    scan.a[i] = grid_scan.a[i];
  }
  Unlock(1);
}


/**
 * Converts an absolute pose to relative pose with respect to 
 * map
 */
Grid::pose CMaCIOccuMapper::toRelativePose(Grid::pose &abs_pose){
  Grid::pose pos=abs_pose;
		
  pos.x -= grid_pose.x;
  pos.y -= grid_pose.y;
		
  return pos;
}
/**
 * Converts a relative pose to absolute pose with respect to 
 * map
 */
Grid::pose CMaCIOccuMapper::toAbsolutePose(Grid::pose &rel_pose){
  Grid::pose pos=rel_pose;
		
  pos.x += grid_pose.x;
  pos.y += grid_pose.y;
		
  return pos;
}

/**
 * Converts an absolute pose to relative pose with respect to 
 * map
 */
Grid::TCoord2D CMaCIOccuMapper::toRelativePose(Grid::TCoord2D &abs_pos){
  Grid::TCoord2D c = abs_pos;
  c.x -= grid_pose.x;
  c.y -= grid_pose.y;
  return c;
}
/**
 * Converts a relative pose to absolute pose with respect to 
 * map
 */
Grid::TCoord2D CMaCIOccuMapper::toAbsolutePose(Grid::TCoord2D &rel_pos){
  Grid::TCoord2D c = rel_pos;
  c.x += grid_pose.x;
  c.y += grid_pose.y;
  return c;
}


/**
 * Get a submap around certain position
 * @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
 * @param pos the metric absolute position around which the map is taken from
 * @param lNx number of pixels in submap in x-dir
 * @param lNy number of pixels in submap in y-dir
 */
void CMaCIOccuMapper::getSubmap(float **data, Grid::TCoord2D pos, int lNx, int lNy){
  if(*data == NULL){
    *data = (float *)malloc(lNx*lNy*sizeof(float));
  }
  Lock(1);
  Grid::TCoord2D relpos(pos.x-grid_pose.x,pos.y-grid_pose.y);
  Unlock(1);
  Grid::TPixel pix = O->toPixel(relpos);
  Lock();
  O->getSubmap(*data, pix,lNx,lNy); ///< Get submap from the current position
  Unlock();
}

void CMaCIOccuMapper::blurMap(float low_prob, float high_prob){
  Lock();
  float *dptr = O->getDataPtr(); ///< Get submap from the current position
  for(int i=0;i<O->size();i++){
    if(dptr[i]>high_prob) dptr[i]=high_prob;
    else if(dptr[i]<low_prob) dptr[i]=low_prob;
  }
  Unlock();
}

void CMaCIOccuMapper::Reset(){
  Lock();
  float *dptr = O->getDataPtr(); ///< Get submap from the current position
  for(int i=0;i<O->size();i++){
    dptr[i] = 0.5;
  }
  Unlock();	
}

/**
 * Get a submap around robot's position
 * @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
 * @param lNx number of pixels in submap in x-dir
 * @param lNy number of pixels in submap in y-dir
 */
void CMaCIOccuMapper::getSubmap(float **data, int lNx, int lNy){
  if(*data == NULL){
    *data = (float *)malloc(lNx*lNy*sizeof(float));
  }
  Lock(1);
  Grid::TCoord2D relpos(relative_pose.x,relative_pose.y);
  Unlock(1);
  Grid::TPixel pix = O->toPixel(relpos);
  Lock();
  O->getSubmap(*data, pix,lNx,lNy); ///< Get submap from the current position
  Unlock();
}

/**
 * Get distance transform of (lNx x lNy) size of block around the robot
 * The pTh is used in binary image thresholding (larger prob equals 1 lower equals 0)
 * @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
 * @param lNx number of pixels in submap in x-dir
 * @param lNy number of pixels in submap in y-dir
 * @param pTh probablity threshold (lower equals 0 higher equals 1)
 * TODO:: Plenty of allocations, should these be optimized... 
 **/
void CMaCIOccuMapper::getDistanceTransform(float **data, int lNx, int lNy, float pTh){
  if(*data == NULL){
    *data = (float *)malloc(lNx*lNy*sizeof(float));
  }
  Lock(1);
  Grid::TCoord2D relpos(relative_pose.x,relative_pose.y);
  Unlock(1);
  Grid::TPixel pix = O->toPixel(relpos);
  Lock();
  O->getSubmap(*data, pix,lNx,lNy); ///< Get submap from the current position
  Unlock();
		
  image<uchar> *input = new image<uchar>(lNx,lNy);
	
  for(int y=0;y<lNy;y++){
    for(int x=0;x<lNx;x++){
      if(*data[lNx*y+x]<pTh) imRef(input,x,y) = 0;
      else imRef(input,x,y) = 1;
    }
  }
  float resolution = O->getResolution();
		
  image<float> *output = dt(input);
  for(int y=0;y<lNy;y++){
    for(int x=0;x<lNx;x++){
      *data[y*lNx+x] = sqrt(imRef(output,x,y))*resolution;
    }
  }
  delete input;
  delete output;
}

/**
 * Get distance transform of (lNx x lNy) size of block around the robot
 * The pTh is used in binary image thresholding (larger prob equals 1 lower equals 0)
 * @param **data pointer to where the data is copied (malloced or if NULL, will be allocated)
 * @param lNx number of pixels in submap in x-dir
 * @param lNy number of pixels in submap in y-dir
 * @param pTh probablity threshold (lower equals 0 higher equals 1)
 * @param *Occu The data pointer of submap is set to Occupancy Grid and latest measurement added to the map
 **/
void CMaCIOccuMapper::getDistanceTransform(float **data, int lNx, int lNy, float pTh,TOccupancyGrid *Occu){
  if(*data == NULL){
    *data = (float *)malloc(lNx*lNy*sizeof(float));
  }
  Lock();
  Lock(1);
  Grid::TCoord2D relpos(relative_pose.x,relative_pose.y);
  Grid::TPixel pix = O->toPixel(relpos);
		
  float *dataptr = Occu->getDataPtr();
		
  O->getSubmap(dataptr, pix,lNx,lNy); ///< Get submap from the current position
		
  ///Reduce the probability of local map to get the dynamic variations.
  for(int i=0;i<Occu->size();i++){
    if(dataptr[i]<0.3) dataptr[i]=0.3;
    else if(dataptr[i]>0.7) dataptr[i]=0.7;
  }
  Occu->setScanFillProb(0, 0 ,current_pose.a, grid_scan,0.01,0.99); 
		
  Unlock();
  Unlock(1);
		
  image<uchar> *input = new image<uchar>(lNx,lNy);
	
  for(int y=0;y<lNy;y++){
    for(int x=0;x<lNx;x++){
      if(dataptr[lNx*y+x]<pTh) imRef(input,x,y) = 0;
      else imRef(input,x,y) = 1;
    }
  }
  float resolution = O->getResolution();
		
  image<float> *output = dt(input);
  for(int y=0;y<lNy;y++){
    for(int x=0;x<lNx;x++){
      *data[y*lNx+x] = sqrt(imRef(output,x,y))*resolution;
    }
  }
  delete input;
  delete output;
}


/***************************************************************
 * Drawing related methods
 ****************************************************************/				
/**
 * adds a pixel to be drawn
 **/
void CMaCIOccuMapper::addDrawPixel(MaCIOccuMapperDrawPixel pix){
  Lock(2);
  drawpixels.push_back(pix);
  Unlock(2);
}
void CMaCIOccuMapper::addDrawPixel(float x, float y, uchar r, uchar g, uchar b){
  Lock(2);
  drawpixels.push_back(MaCIOccuMapperDrawPixel(x,y,r,g,b));
  Unlock(2);
}
void CMaCIOccuMapper::clearDrawPixelVector(){
  Lock(2);
  drawpixels.clear();
  Unlock(2);
}
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/// Thread function and related
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
void CMaCIOccuMapper::stopOccuMapperExecution(){
  dPrint(1,"Stopping the Thread execution");
  threadRunning = false;
		
  if(WaitThread()!=-1){
    dPrint(1,"Thread Succesfully stopped");
  }else{
    dPrint(1,"An Error occurred while stopping the thread");
  }
		
}

int CMaCIOccuMapper::ThreadFunction(int num){
  threadRunning = true;
		
  // Variables for accessing position and ranging
  Position::CPositionData pd_timed;
  Ranging::CRangingData rd;
  int rangingSeq = -1;
			
  bool rr,pr;
  gim::time measure_diff;
  unsigned long int cnt=0;
  int imagecnt = 0;
  int mapcnt=0;
		

  int Nx,Ny;
  O->getDim(Nx,Ny); ///< Dimensions of the OccupancyGrid
  float *tmp_data = (float *) malloc(O->size()*sizeof(float));
		
		
  while(threadRunning){
    rr = rangingClients[0]->GetDistanceArray(rd, &rangingSeq, 1000);
    if (!rr) {
      dPrint(1,"Failed to get DistanceArray?");
      continue;
    }
		
    if(rd.GetDistanceElementCount() >= enough_scan_points){
      dPrint(1,"Enough Was not Enough - reallocating the scan to %d scans",rd.GetDistanceElementCount());
      grid_scan.alloc(rd.GetDistanceElementCount());
      enough_scan_points = rd.GetDistanceElementCount();
    }
				
    // This sleep is to verify that we already have a position
    // matching the Distance. 
    ownSleep_ms(10);    
				
    // Get best time-matching position for the received DistanceArray
    const Common::TTimestamp *ts = rd.GetTimestamp(); // Timestamp from distancearray
    if (ts){
      pr = positionClient->GetPositionEvent(pd_timed, ts->GetGimTime(), &measure_diff);
      if (!pr){
	dPrint(1,"Failed to get PositionEvent (timed)?");
	continue;
      }
    }
    ////////////////////////////////////////////////////////////////////
    // Calc calc calc, pos pos pos, magic magic magic ...
    // More Calc calc calc, pos pos pos, magic magic magic ...
    ////////////////////////////////////////////////////////////////////
    cnt++;
    const Position::TPose2D *odo; ///< Odometry from the robot		
    const MaCI::Ranging::TDistance *r_meas;
    uint32_t movementHitCount = 0;
    const MaCI::Ranging::TDistance* movementHits[rd.GetDistanceElementCount()];
    odo = pd_timed.GetPose2D();  ///< pointer to odometry data
    if(odo == NULL){ fprintf(stderr,"Odo NULL?!?!?\n");	exit(1);}
		
    ///Initialization
    if(cnt==1){
      grid_pose.set(odo->x,odo->y,0);	
    }
    Lock(1);
    current_pose.set(odo->x,odo->y,odo->a);
    relative_pose.set(odo->x-grid_pose.x,odo->y-grid_pose.y, odo->a); 
				
    for(int i=0;i<rd.GetDistanceElementCount();i++){
      r_meas = rd.GetDistance(i);
      if(r_meas->distance > min_laser_distance)	grid_scan.r[i] = r_meas->distance;	
      else grid_scan.r[i] = 0;	
      grid_scan.a[i] = r_meas->angle;	
					
      if (bSendMotionRanges){
	Grid::TCoord2D lpos(relative_pose.x + r_meas->distance*cos(r_meas->angle+odo->a),
						relative_pose.y + r_meas->distance*sin(r_meas->angle+odo->a));
	float v;
	O->getValue(lpos,v);
	///if(v ei täsmää riittävästi) grid_scan.r[i] = 0; (ja talleta piste toiseen skanniin)
	//dPrint(1,"scan %i", i);
						
	if (v < 0.05 && r_meas->distance < max_laser_distance){
	 // dPrint(1,"Adding new movement hit %i (%f) %f (%f,%f)", i, v,r_meas->distance, lpos.x, lpos.y );
	  //grid_scan.r[i] = 0;
	  movementHits[movementHitCount] = r_meas;
	  movementHitCount++;
	}
      }
    }
    grid_scan.N = rd.GetDistanceElementCount();

    if (movementHitCount > 0 && bSendMotionRanges){
      //dPrint(1,"Constructing rang header");
      //      if (bSendMotionRanges) dPrint(1,"True");
      int beam_count = 100;
      float beam_width = 0.001;
      float distance = 0.001;
      float angle = 0.001;

      // construct the scan of mobile objects
      MaCI::Ranging::CRangingData movementScan;
	  //MaCI::Ranging::TDeviceInformationPosition *lPose = ;
	  //MaCI::Ranging::TDeviceInformationPosition dPose()
      //dPrint(1,"step 1");
      //gim::binbag::CBinBag bag;
      //dPrint(1,"step 2");
      //movementScan.EncodeTo(&bag);
      movementScan.CreateInternalBinBag();
      //dPrint(1,"step 3");
					
      const Ranging::TDistanceHeader *header;
      //dPrint(1,"step 4");
      header = rd.GetDistanceHeader();
      //dPrint(1,"step 5");
      //Ranging::TDistanceHeader movHeader(header->beam_width, (uint32_t)movementHitCount);
      //dPrint(1,"step 6");
      movementScan.SetTimestamp(*rd.GetTimestamp());
      //movementScan.SetTimestamp(MaCI::Common::TTimestamp(0, 0));
      //dPrint(1,"step 7");
      //movementScan.SetDistanceHeader(MaCI::Ranging::TDistanceHeader(beam_width, beam_count));
      movementScan.SetDistanceHeader(MaCI::Ranging::TDistanceHeader(header->beam_width, movementHitCount));
                //		dPrint(1,"step 8 %f, %u",header->beam_width, movementHitCount);
      //movementScan.SetDeviceInformationLimits(*rd.GetDeviceInformationLimits());
      //dPrint(1,"step 9");
      //movementScan.SetDeviceInformationPosition(*rd.GetDeviceInformationPosition());
      //dPrint(1,"adding rang data");
					
      					for(int i=0;i<movementHitCount;i++){
      						movementScan.AddDistance(MaCI::Ranging::TDistance(movementHits[i]->angle, movementHits[i]->distance));
      					}
					
      //send it away
      //dPrint(1,"Sending it away");
      rangingServer->SendDistanceArrayEvent(movementScan);
	//sendRanges(movementHitCount, movementHits);
      //dPrint(1,"and done !!!!");
					
    }
    Unlock(1);
				
    Lock();
				
    ///< set the current measurement to global map                              vapaa, varattu 
    O->setScanFillProb(relative_pose.x,relative_pose.y,relative_pose.a, grid_scan,0.2,0.9, max_laser_distance);
    Unlock();
				
    /// Check if we are at the border of the grid
    float toPos = sqrt(relative_pose.x*relative_pose.x+relative_pose.y*relative_pose.y);
    float toEdge = occu_map_size/2.0-toPos;
    if(toEdge < EdgeThreshold){ ///Time to remap
      grid_pose.set(odo->x,odo->y,0);
      Grid::TPixel p = O->toPixel(Grid::TCoord2D(relative_pose.x,relative_pose.y)); 
      relative_pose.set(0,0,odo->a);
      O->getSubmap(tmp_data, p,Nx,Ny); ///< Get submap from the current position
      O->setGridToValue(0.5); ///< initialize the grid
      dPrint(10,"(%d %d), (%d %d)",Nx,Ny,lrint((double) Nx/2.0),lrint((double) Ny/2.0) );
      O->setSubmap(tmp_data, Grid::TPixel(lrint((double) Nx/2.0)-1 ,lrint((double) Ny/2.0)-1), Nx, Ny);	
    }

    if(imageServerActivated){
      if(imagecnt > imageSendingFrequency && imageServer->GetInterfaceSubscriberCount()>0){ 
	sendMapImage(relative_pose);
	imagecnt=0;
      }else{ 
	imagecnt++;
      }				
    }
				
    if(mapServerActivated){
      if(mapcnt>mapSendingFrequency){
	sendMap(grid_pose,relative_pose);
	mapcnt = 0;
      }else{
	mapcnt++;
      }
    }
  }
  free(tmp_data);
  return 0;
}
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/// Debug Main
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
#ifdef USE_MACI_OCCUMAPPER_DEBUG
int main(){
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(5);
  debugSetLogFilename("MaCI_OccuMapper-debug.log");
		
  // 		CMaCIOccuMapper om("130.233.120.103",50002,"","J2B2Sim1","Motion","Laser"
  // 																														,15,0.05,5.0,400, 80.0, 0.01);
  // 		
  // 		//CMaCIOccuMapper om("rousku.dyndns.tv",50002,"Map_tester","J2B2","Motion","Laser"
  // 		//																												,50,0.1,10.0,400, 80.0, 0.01);
  // 		om.activateImageServer("LeMapImage", 
  // 													 true, 
  // 													 true,
  // 													 0.1,
  // 													 true);
  // 													 
  // 		om.activateMapServer("GridMap",true, 8.0);
  // 		
  CMaCIOccuMapper om2("130.233.120.103",50006,"tester","Rolloottori","ODO","ODO"
		      ,15,0.25,5.0,800, 4.5, 0.1);

				
  ownSleep_ms(1000);
				
  om2.activateRangingServer("FilLaser"); 
  om2.activateImageServer("LeMapImage", true, true, 0.1, true);													 
  //		om2.activateMapServer("GridMap",true, 8.0);
  /*		
		CMaCIOccuMapper om3("130.233.120.103",50002,"","J2B2","GLOC-Pose","GLOC-Laser"
		,15,0.05,5.0,400, 80.0, 0.01);
		

													 
		om3.activateMapServer("GridMap",true, 8.0);
		
  */
		
  while(1){
    //Grid::pose pos;
    //om.getCurrentPose(pos);
    //om.addDrawPixel(pos.x,pos.y,255,0,0);
    ownSleep_ms(1000);
  }
		
		
		
  //om.stopOccuMapperExecution();
		
  return 0;
}

#endif
